﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using System.Diagnostics;

namespace VRP_GTS
{
    class Heuristics
    {
         // penalty parameters
        public static double alphaMin = 1;
        public static double alphaMax = 6400;
        public static double alphaCap = 100;

        public static double objValueOfBestMove;    
        //
        public static Path routeR;
        public static Path routeS;
        public static Path originalRouteR;
        public static Path originalRouteS;
        //
        public static Node nodeI;
        public static Node nodeJ;

        //
        public static List<Node> nodeListOfBestNewRouteR;
        public static List<Node> nodeListOfBestNewRouteS;
        public static List<Link> removedLinksOfBestMove;

        //
        public static double currentTotalCostWithPenalty;

        // tabu links initialization 
        // first paramter stores the tabu link, the second paramter stores tabu tenure which is a randomly generated number in a range
        // when the link is first added into tabuLinks
        public static Dictionary<Link, int> tabuLinks = new Dictionary<Link, int>();
        
        public static List<Link> candidateLinks;

        public static CVRPSolution bestFeasibleSolution;

        public static CVRPSolution GTS(CVRPData vrpData, CVRPSolution initSolution)
        {
            int numOfCustomers = vrpData.Dimension - 1;
            int numOfVehicles = initSolution.TourList.Count; ;
            double capacity = vrpData.Capacity;
            Link[,] linkMatrix = vrpData.LinkMatrix;

            // sparsification factor beta and betaDiv
            double beta = 1.25;
            double betaDiv = 1.75;

            if (numOfCustomers < 100)
            {
                beta = 1.75;
                betaDiv = 2.25;
            }

            // tabu tenure threshold
            int tMin = 5;
            int tMax = 10;

            System.Random RandNum = new System.Random();

            // iteration monitor
            int divIterMax = 10 * numOfCustomers;
            int numDivIter = 200;
            //int stopIterMax = 15 * numOfCustomers;
            int stopIterMax = 1000000000;
            int feasibleIterMax = 10;
            int overallIterMax = 1000000000;

            //Console.ReadKey();
            //Console.WriteLine("started");

            // could be added if the number of vehicles constraint is considered 
            // modify intial solution to satisfy the number of vehicles constraint
            // initSolution.ModifySolution();

            CVRPSolution currentSolution = initSolution;
            double currentTotalCost = currentSolution.TotalCost;
            
            currentTotalCostWithPenalty = currentSolution.GetTotalCostWithPenalty(capacity);

            bestFeasibleSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentSolution);
            CVRPSolution bestSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentSolution);                  // might be feasible or not


            // iteration initialization
            int stopIter = 0;
            int divIter = 0;
            int feasibleIter = 0;
            int feasibleCount = 0;
            int divCount = 0;
            int divFlag = 0;
            int overallIter = 0;

            // candidate links initialization
            double initTotalCost = initSolution.TotalCost;
     

            double granThreshold = GetGranThreshold(initTotalCost, numOfCustomers, numOfVehicles, beta);
            candidateLinks = GetCandidateLinks(linkMatrix, granThreshold);


            // Time limitation
            Stopwatch timer = new Stopwatch();            // get running time
            timer.Start();                                // start to calculate time    
         
            while (overallIter < overallIterMax && stopIter < stopIterMax && timer.ElapsedMilliseconds <= 60000*4)
            {
                objValueOfBestMove = Double.PositiveInfinity; // the objective value with penalty of the best move
                originalRouteR = null;                        // needs to be reomved from the current solution
                originalRouteS = null;                        // needs to be reomved from the current solution
                nodeListOfBestNewRouteR = new List<Node>();   // needs to create a new route R using nodelist in the next solution
                nodeListOfBestNewRouteS = new List<Node>();   // needs to create a new route S using nodelist in the next solution
                removedLinksOfBestMove = new List<Link>();    // links removed from the current solution, needs to be added into the tabuLinks

                foreach (Link candidateLink in candidateLinks)
                {
                    // if candidateLink is in the tabuLinks, continue to next candidateLink
                    if (tabuLinks.ContainsKey(candidateLink))
                    {
                        continue;
                    }

                    nodeI = candidateLink.Tail;
                    nodeJ = candidateLink.Head;

                    routeR = nodeI.onPath;
                    routeS = nodeJ.onPath;

                    if (routeS == null) // means nodeJ is depot, cannot perform TwoExchange
                    {
                        continue;
                    }

                    if (routeR == null) // means nodeI is depot
                    {
                        foreach (Path routeRR in currentSolution.TourList)
                        {
                            if (routeRR != routeS)
                            {
                                TryTwoExchange(routeRR, capacity, linkMatrix);
                                TryThreeExchange2(routeRR, capacity, linkMatrix);

                            }
                        }
                    }
                    else  // neither nodeI nor nodeJ is depot
                    {
                        if (routeR != routeS) // means candidateLink is not in the current solution
                        {
                            TryTwoExchange(routeR, capacity, linkMatrix);

                            TryThreeExchange1(routeR, capacity, linkMatrix);

                            TryThreeExchange2(routeR, capacity, linkMatrix);

                            TryFourExchange(routeR, capacity, linkMatrix);
                        }
                    }
                }

                // Update the current solution to the best move solution, note that the best move solution
                // may not improve the best feasible solution or the best solution (feasible or not)
                currentSolution.TourList.Remove(originalRouteR);
                currentSolution.TourList.Remove(originalRouteS);
                if (nodeListOfBestNewRouteR.Count > 2) 
                // if count == 2, means the routes only contains two depot node which is not valid, in this case, the num of routes decreases by 1
                {
                    currentSolution.TourList.Add(new Path(nodeListOfBestNewRouteR));
                }
                if (nodeListOfBestNewRouteS.Count > 2)
                {
                    currentSolution.TourList.Add(new Path(nodeListOfBestNewRouteS));
                }                
                currentTotalCost = currentSolution.GetTotalCost();
                currentTotalCostWithPenalty = currentSolution.GetTotalCostWithPenalty(capacity);

                //Console.WriteLine(currentTotalCost);
                //currentSolution.CheckFeasibility(vrpData);


                // Update the tabuLinks, first decrease the tenure value of each tabu link by 1,
                // then remove the links whose value drop to 0,
                // at last, add the newly removed links from the current solution
                // decrease the value of each tabu link by 1
                //Console.WriteLine(tabuLinks.Count);
                foreach (var key in tabuLinks.Keys.ToList())
                {
                    tabuLinks[key]--;
                }

                // remove the tabu link with value = 0, these links will be available for re-inserting
                foreach (var kvp in tabuLinks.Where(pair => pair.Value <= 0).ToList())
                {
                    tabuLinks.Remove(kvp.Key);
                }

                foreach (Link link in removedLinksOfBestMove)
                {
                    if (!(tabuLinks.ContainsKey(link)))
                    {
                        tabuLinks.Add(link, RandNum.Next(tMin, tMax));
                    }
                                       
                    // add the removed link into the candidateLinks if the link is not included yet
                    if (!(candidateLinks.Contains(link)))
                        candidateLinks.Add(link);
                }         

                // Update the bestFeasibleSolution, bestSolution (feasible or not)
                // first check if the current solution is feasible or not, then update accordingly
                // If the best solutoins are updated, set stopIter = 0, divIter = 0

                int flagImprovedSolution = 0;

                // update best solution
                if (currentSolution.TotalCostWithPenalty < bestSolution.TotalCostWithPenalty)
                {
                    flagImprovedSolution = 1;
                    bestSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentSolution);
                }

                feasibleIter++;

                if (currentSolution.CheckCapacityConstraint(capacity) == 0)
                {
                    feasibleCount++;
                    if (currentSolution.TotalCost < bestFeasibleSolution.TotalCost)
                    {
                        flagImprovedSolution = 1;
                        bestFeasibleSolution = ProgramEntry.DeepCopy<CVRPSolution>(currentSolution);
                        Console.WriteLine("found better solution " + bestFeasibleSolution.TotalCost);
                    }
                }

                if (flagImprovedSolution == 1)
                {
                    stopIter = 0;
                    divIter = 0;
                }
                else
                {
                    // update iteration counters
                    divIter++;     
                    stopIter++;
                }

                // If after divIterMax iterations, the current best solution is not improved, 
                // the sparsification factor is increased to betaDiv,
                // a new sparse graph is determined.
                if(divIter == divIterMax)
                {
                    //diversification: increase beta value to 1.75 to enlarge CandidateLinks
                    granThreshold = GetGranThreshold(initTotalCost, numOfCustomers, numOfVehicles, betaDiv);
                    candidateLinks = GetCandidateLinks(linkMatrix, granThreshold);
                    divIter = 0;
                    divFlag = 1;
                }

                if (divFlag == 1)
                {
                    divCount++;
                }

                if (divCount == numDivIter)
                {
                    //set beta value back to 1.25 after numDivIter iterations
                    granThreshold = GetGranThreshold(initTotalCost, numOfCustomers, numOfVehicles, beta);
                    candidateLinks = GetCandidateLinks(linkMatrix, granThreshold);
                    divCount = 0;
                    divFlag = 0;
                }

                // check if we need to update the penalty parameters
                // Every 10 (feasibleIterMax) iterations, if all the previous 10 visited solutions were feasible  with respect to
                // the capacity constraints, then alphaCap is set to max[alphamin, alpha/2], whereas if all were infeasible
                // it is set to min[alphaMax, 2alpha]
                if (feasibleIter == feasibleIterMax)
                {
                    feasibleIter = 0;
                    if (feasibleCount == 10)
                    {
                        alphaCap = Math.Max(alphaMin, alphaCap / 2);
                    }
                    if (feasibleCount == 0)
                    {
                        alphaCap = Math.Min(alphaMax, 2 * alphaCap);
                    }
                    feasibleCount = 0;
                }

                overallIter++;
         }

            Console.WriteLine("Granular tabu search done.\nTotalcost : " + bestFeasibleSolution.TotalCost);
            return bestFeasibleSolution; // may be feasible or not
        }

        public static CVRPSolution SavingsMethod(CVRPData vrpData)
        {
            vrpData.CreateNodeList();
            List<Node> nodeList = vrpData.NodeList;
            double capacity = vrpData.Capacity;
            double? bestKnown = vrpData.BestKnownSolution;
            int n = vrpData.Dimension;

            // Initialize the routes, each customer node forms a route with the depot
            List<Path> routeList = new List<Path>();                                                // define a route list
            for (int i = 1; i < nodeList.Count; i++)                                                // for each customer node  
            {
                routeList.Add(new Path(new List<Node>() { nodeList[0], nodeList[i], nodeList[0] }));// create route list
            }

            // Initialize the savings list for each node pair (i,j)
            List<Savings> savingsList = new List<Savings>();                                        // define a saving list
            for (int i = 1; i < nodeList.Count - 1; i++)
            {
                for (int j = i + 1; j < nodeList.Count; j++)
                {
                    savingsList.Add(new Savings(i, j, nodeList[0], nodeList[i], nodeList[j]));      // create saving list
                }
            }

            // Sort the savings list in decreasing order
            var sortedSavingsList = from element in savingsList
                                    orderby element.S descending
                                    select element;

            // Combine routes
            Path r1 = null;                                                                         // route includes node i 
            Path r2 = null;                                                                         // route includes node j
            Node ndi = null;                                                                        // node list i
            Node ndj = null;                                                                        // node list j

            foreach (Savings sij in sortedSavingsList)
            {
                int i = sij.I;                                                                      // i of node pair (i,j) on saving list
                int j = sij.J;                                                                      // j of node pair (i,j) on saving list

                ndi = nodeList[i];
                ndj = nodeList[j];

                r1 = ndi.onPath;
                r2 = ndj.onPath;

                //check if node i and node j belong to different routes, and the load of two routes is less than the capacity
                if (r1 != r2 && (r1.Load + r2.Load) <= capacity)
                {
                    int index1 = r1.AllNodes.Count;                                                 // the count of all nodes on route1
                    int index2 = r2.AllNodes.Count;                                                 // the count of all nodes on route2

                    //check if node i is the first node of the path
                    if (ndi == r1.AllNodes[1])
                    {
                        //check if node j is the first node of the path
                        if (ndj == r2.AllNodes[1])
                        {
                            r1.AllNodes.RemoveAt(0);                                                // remove the node with index = 0 on route1
                            r1.AllNodes.Reverse();                                                  // reverse the order of the rest nodes on route1
                            r2.AllNodes.RemoveAt(0);                                                // remove the node with index = 0 on route2

                            routeList.Remove(r1);                                                   // remove route1 (arc (0,i))from route list
                            routeList.Remove(r2);                                                   // remove route2 (arc (j,0))from route list
                            routeList.Add(new Path(r1.AllNodes.Concat(r2.AllNodes).ToList()));      // add a new path (i,j)
                            continue;
                        }
                        //check if node j is the last node of the path
                        if (ndj == r2.AllNodes[index2 - 2])
                        {
                            r2.AllNodes.RemoveAt(index2 - 1);                                       // remove the node with index = index2-1 on route2
                            r1.AllNodes.RemoveAt(0);                                                // remove the node with index = 0 on route1

                            routeList.Remove(r1);                                                   // remove r1 (arc (0,i))from route list
                            routeList.Remove(r2);                                                   // remove r2 (arc (j,0))from route list
                            routeList.Add(new Path(r2.AllNodes.Concat(r1.AllNodes).ToList()));      // add a new path (j,i)
                            continue;
                        }
                    }
                    //check if node i is the last node of the path
                    if (ndi == r1.AllNodes[index1 - 2])
                    {
                        //check if node j is the first node of the path
                        if (ndj == r2.AllNodes[1])
                        {
                            r1.AllNodes.RemoveAt(index1 - 1);
                            r2.AllNodes.RemoveAt(0);

                            routeList.Remove(r1);
                            routeList.Remove(r2);
                            routeList.Add(new Path(r1.AllNodes.Concat(r2.AllNodes).ToList()));
                            continue;
                        }
                        //check if node j is the last node of the path
                        if (ndj == r2.AllNodes[index2 - 2])
                        {
                            r1.AllNodes.RemoveAt(index1 - 1);
                            r2.AllNodes.Reverse();
                            r2.AllNodes.RemoveAt(0);

                            routeList.Remove(r1);
                            routeList.Remove(r2);
                            routeList.Add(new Path(r1.AllNodes.Concat(r2.AllNodes).ToList()));
                        }
                    }
                }
            }

            double totalCost = 0;

            for (int i = 0; i < routeList.Count; i++)
                totalCost += routeList[i].Length;

            CVRPSolution solution = new CVRPSolution();
            solution.TotalCost = totalCost;
            solution.TourList = routeList;

            Console.WriteLine("Savings methods done.\nTotalcost : " + totalCost);
            return solution;
        }

        public static CVRPSolution RandomizedSavingsMethod(CVRPData vrpData)
        {
            vrpData.CreateNodeList();
            List<Node> nodeList = vrpData.NodeList;
            double capacity = vrpData.Capacity;
            double? bestKnown = vrpData.BestKnownSolution;
            int n = vrpData.Dimension;

            // Initialize the routes, each customer node forms a route with the depot
            List<Path> routeList = new List<Path>();                                                // define a route list
            for (int i = 1; i < nodeList.Count; i++)                                                // for each customer node  
            {
                routeList.Add(new Path(new List<Node>() { nodeList[0], nodeList[i], nodeList[0] }));// create route list
            }

            // Initialize the savings list for each node pair (i,j)
            List<Savings> savingsList = new List<Savings>();                                        // define a saving list
            for (int i = 1; i < nodeList.Count - 1; i++)
            {
                for (int j = i + 1; j < nodeList.Count; j++)
                {
                    savingsList.Add(new Savings(i, j, nodeList[0], nodeList[i], nodeList[j]));      // create saving list
                }
            }

            
            //Return routes created by randomized savings list
            Random rand = new Random();
            // Sort the savings list in decreasing order
            //var sortedSavingsList = from element in savingsList
            //                        orderby element.S descending
            //                        select element;

            List<Savings> sortedSavingsList = savingsList.OrderByDescending(x => x.S).ToList();

            // Combine routes
            Path r1 = null;                                                                         // route includes node i 
            Path r2 = null;                                                                         // route includes node j
            Node ndi = null;                                                                        // node list i
            Node ndj = null;                                                                        // node list j

            while (sortedSavingsList.Count() > 0)
            {

                // Generate a Poisson Random Variable with mean 1

                double p = 1, L = Math.Exp(-1);
                int k = 0;
                do
                {
                    k++;
                    p = rand.NextDouble();
                    
                }
                while (p > L);
                int K = Math.Min(k - 1, sortedSavingsList.Count() - 1);

               // K = 0;

                Savings sij = sortedSavingsList[K];


                int i = sij.I;                                                                      // i of node pair (i,j) on saving list
                int j = sij.J;                                                                      // j of node pair (i,j) on saving list

                ndi = nodeList[i];
                ndj = nodeList[j];

                r1 = ndi.onPath;
                r2 = ndj.onPath;

                //check if node i and node j belong to different routes, and the load of two routes is less than the capacity
                if (r1 != r2 && (r1.Load + r2.Load) <= capacity)
                {
                    int index1 = r1.AllNodes.Count;                                                 // the count of all nodes on route1
                    int index2 = r2.AllNodes.Count;                                                 // the count of all nodes on route2

                    //check if node i is the first node of the path
                    if (ndi == r1.AllNodes[1])
                    {
                        //check if node j is the first node of the path
                        if (ndj == r2.AllNodes[1])
                        {
                            r1.AllNodes.RemoveAt(0);                                                // remove the node with index = 0 on route1
                            r1.AllNodes.Reverse();                                                  // reverse the order of the rest nodes on route1
                            r2.AllNodes.RemoveAt(0);                                                // remove the node with index = 0 on route2

                            routeList.Remove(r1);                                                   // remove route1 (arc (0,i))from route list
                            routeList.Remove(r2);                                                   // remove route2 (arc (j,0))from route list
                            routeList.Add(new Path(r1.AllNodes.Concat(r2.AllNodes).ToList()));      // add a new path (i,j)
                            continue;
                        }
                        //check if node j is the last node of the path
                        if (ndj == r2.AllNodes[index2 - 2])
                        {
                            r2.AllNodes.RemoveAt(index2 - 1);                                       // remove the node with index = index2-1 on route2
                            r1.AllNodes.RemoveAt(0);                                                // remove the node with index = 0 on route1

                            routeList.Remove(r1);                                                   // remove r1 (arc (0,i))from route list
                            routeList.Remove(r2);                                                   // remove r2 (arc (j,0))from route list
                            routeList.Add(new Path(r2.AllNodes.Concat(r1.AllNodes).ToList()));      // add a new path (j,i)
                            continue;
                        }
                    }
                    //check if node i is the last node of the path
                    if (ndi == r1.AllNodes[index1 - 2])
                    {
                        //check if node j is the first node of the path
                        if (ndj == r2.AllNodes[1])
                        {
                            r1.AllNodes.RemoveAt(index1 - 1);
                            r2.AllNodes.RemoveAt(0);

                            routeList.Remove(r1);
                            routeList.Remove(r2);
                            routeList.Add(new Path(r1.AllNodes.Concat(r2.AllNodes).ToList()));
                            continue;
                        }
                        //check if node j is the last node of the path
                        if (ndj == r2.AllNodes[index2 - 2])
                        {
                            r1.AllNodes.RemoveAt(index1 - 1);
                            r2.AllNodes.Reverse();
                            r2.AllNodes.RemoveAt(0);

                            routeList.Remove(r1);
                            routeList.Remove(r2);
                            routeList.Add(new Path(r1.AllNodes.Concat(r2.AllNodes).ToList()));
                        }
                    }
                }
                sortedSavingsList.RemoveAt(K);
            }


            double totalCost = 0;

            for (int i = 0; i < routeList.Count; i++)
                totalCost += routeList[i].Length;

            CVRPSolution solution = new CVRPSolution();
            solution.TotalCost = totalCost;
            solution.TourList = routeList;

           // Console.WriteLine("Savings methods done.\nTotalcost : " + totalCost);
           // solution.CheckFeasibility(vrpData);
            return solution;
        }
        
        public static double GetGranThreshold(double totalCost, int numOfCustomers, int numOfVehicles, double beta)
        {
            double granThreshold = beta * totalCost / (numOfCustomers + numOfVehicles);
            return granThreshold;
        }

        public static List<Link> GetCandidateLinks(Link[,] linkMatrix, double granThreshold)
        {
            List<Link> candidateLinks = new List<Link>();

            //  Add the short arcs and arcs incident to the depot into the candidate links 
            for (int i = 0; i < linkMatrix.GetLength(0); i++)
            {
                for (int j = i + 1; j < linkMatrix.GetLength(0); j++)
                {
                    if (i == 0 || linkMatrix[i, j].Length <= granThreshold)
                    {
                        candidateLinks.Add(linkMatrix[i, j]);
                        candidateLinks.Add(linkMatrix[j, i]);
                    }
                }
            }

            return candidateLinks;
        }

        public static double GetLengthFromNodeList(List<Node> nodeList)
        {
            double length = 0;
            for (int i = 0; i < nodeList.Count - 1; i++)
                length += Link.GetLength(nodeList[i], nodeList[i + 1]);

            return length;
        }

        public static double GetLoadFromNodeList(List<Node> nodeList)
        {
            double load = 0;
            for (int i = 0; i < nodeList.Count; i++)
                load += nodeList[i].Demand;

            return load;
        }

        static void GetTempObjValueOfCurrentMove(Path routeR, double capacity, out double tempObjValueOfCurrentMove)
        {
            tempObjValueOfCurrentMove = currentTotalCostWithPenalty - routeR.Length - routeS.Length;
            tempObjValueOfCurrentMove -= alphaCap * Math.Max(0, routeR.Load - capacity);
            tempObjValueOfCurrentMove -= alphaCap * Math.Max(0, routeS.Load - capacity);
        }

        public static void TryTwoExchange(Path routeR, double capacity, Link[,] linkMatrix)
        {
            double tempObjValueOfCurrentMove;
            GetTempObjValueOfCurrentMove(routeR, capacity, out tempObjValueOfCurrentMove);
            // store the objective value with penalty of the current move
            double objValueOfCurrentMove;

            int indexOfNodeI = routeR.AllNodes.IndexOf(nodeI);
            int indexOfNodeJ = routeS.AllNodes.IndexOf(nodeJ);


            List<Link> otherAddedLinks = new List<Link>();
            otherAddedLinks.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, routeR.AllNodes[indexOfNodeI + 1].Id]);

            foreach (Link link in otherAddedLinks)
            {
                if (link == null)
                    continue;

                if (tabuLinks.ContainsKey(link) || !candidateLinks.Contains(link))
                    return;
            }

            // create two node list to hold the new routes after performing node exchange mechanisms
            //List<Link> removedLinksOfCurrentMove = new List<Link>();
            List<Node> nodeListOfNewRouteR = new List<Node>();
            List<Node> nodeListOfNewRouteS = new List<Node>();
            double lengthOfNewRouteR;
            double lengthOfNewRouteS;
            double loadOfNewRouteR;
            double loadOfNewRouteS;
            List<Link> removedLinksOfCurrentMove = new List<Link>();

            // add removed links to removedLinksOfCurrentMove
            removedLinksOfCurrentMove.Add(linkMatrix[nodeI.Id, routeR.AllNodes[indexOfNodeI + 1].Id]);
            removedLinksOfCurrentMove.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, nodeJ.Id]);

            // performing Two Exchange
            for (int i = 0; i < indexOfNodeI + 1; i++)
                nodeListOfNewRouteR.Add(routeR.AllNodes[i]);
            for (int i = indexOfNodeJ; i < routeS.AllNodes.Count; i++)
                nodeListOfNewRouteR.Add(routeS.AllNodes[i]);

            for (int i = 0; i < indexOfNodeJ; i++)
                nodeListOfNewRouteS.Add(routeS.AllNodes[i]);
            for (int i = indexOfNodeI + 1; i < routeR.AllNodes.Count; i++)
                nodeListOfNewRouteS.Add(routeR.AllNodes[i]);

            lengthOfNewRouteR = GetLengthFromNodeList(nodeListOfNewRouteR);
            lengthOfNewRouteS = GetLengthFromNodeList(nodeListOfNewRouteS);

            loadOfNewRouteR = GetLoadFromNodeList(nodeListOfNewRouteR);
            loadOfNewRouteS = GetLoadFromNodeList(nodeListOfNewRouteS);

            objValueOfCurrentMove = tempObjValueOfCurrentMove + lengthOfNewRouteR + lengthOfNewRouteS
                + alphaCap * Math.Max(0, loadOfNewRouteR - capacity) + alphaCap * Math.Max(0, loadOfNewRouteS - capacity);

            // update the best move
            if (objValueOfCurrentMove < objValueOfBestMove)
            {
                objValueOfBestMove = objValueOfCurrentMove;
                originalRouteR = routeR;
                originalRouteS = routeS;
                nodeListOfBestNewRouteR = nodeListOfNewRouteR;
                nodeListOfBestNewRouteS = nodeListOfNewRouteS;
                removedLinksOfBestMove = removedLinksOfCurrentMove;
            }
        }

        public static void TryFourExchange(Path routeR, double capacity, Link[,] linkMatrix)
        {
            double tempObjValueOfCurrentMove;
            GetTempObjValueOfCurrentMove(routeR, capacity, out tempObjValueOfCurrentMove);

            // store the objective value with penalty of the current move
            double objValueOfCurrentMove;

            int indexOfNodeI = routeR.AllNodes.IndexOf(nodeI);
            int indexOfNodeJ = routeS.AllNodes.IndexOf(nodeJ);

            if ((routeR.AllNodes.Count - indexOfNodeI) < 3)
            {
                return;
            }

            List<Link> otherAddedLinks = new List<Link>();
            otherAddedLinks.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, routeR.AllNodes[indexOfNodeI + 1].Id]);
            otherAddedLinks.Add(linkMatrix[nodeJ.Id, routeR.AllNodes[indexOfNodeI + 2].Id]);
            otherAddedLinks.Add(linkMatrix[routeR.AllNodes[indexOfNodeI + 1].Id, routeS.AllNodes[indexOfNodeJ + 1].Id]);

            foreach (Link link in otherAddedLinks)
            {
                if (link == null)
                    continue;

                if (tabuLinks.ContainsKey(link) || !candidateLinks.Contains(link))
                    return;
            }

            // create two node list to hold the new routes after performing node exchange mechanisms
            //List<Link> removedLinksOfCurrentMove = new List<Link>();
            List<Node> nodeListOfNewRouteR = new List<Node>();
            List<Node> nodeListOfNewRouteS = new List<Node>();
            double lengthOfNewRouteR;
            double lengthOfNewRouteS;
            double loadOfNewRouteR;
            double loadOfNewRouteS;
            List<Link> removedLinksOfCurrentMove = new List<Link>();

            // add removed links to removedLinksOfCurrentMove
            removedLinksOfCurrentMove.Add(linkMatrix[nodeI.Id, routeR.AllNodes[indexOfNodeI + 1].Id]);
            removedLinksOfCurrentMove.Add(linkMatrix[routeR.AllNodes[indexOfNodeI + 1].Id, routeR.AllNodes[indexOfNodeI + 2].Id]);
            removedLinksOfCurrentMove.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, nodeJ.Id]);
            removedLinksOfCurrentMove.Add(linkMatrix[nodeJ.Id, routeS.AllNodes[indexOfNodeJ + 1].Id]);
            if (removedLinksOfCurrentMove.Contains(null))
                Console.WriteLine("Null link");

            // performing Two Exchange
            for (int i = 0; i < routeR.AllNodes.Count; i++)
            {
                if (i == (indexOfNodeI + 1))
                {
                    nodeListOfNewRouteR.Add(routeS.AllNodes[indexOfNodeJ]);
                }
                else
                {
                    nodeListOfNewRouteR.Add(routeR.AllNodes[i]);
                }
            }


            for (int i = 0; i < routeS.AllNodes.Count; i++)
            {
                if (i == indexOfNodeJ)
                {
                    nodeListOfNewRouteS.Add(routeR.AllNodes[indexOfNodeI + 1]);
                }
                else
                {
                    nodeListOfNewRouteS.Add(routeS.AllNodes[i]);
                }
            }
            
            lengthOfNewRouteR = GetLengthFromNodeList(nodeListOfNewRouteR);
            lengthOfNewRouteS = GetLengthFromNodeList(nodeListOfNewRouteS);

            loadOfNewRouteR = GetLoadFromNodeList(nodeListOfNewRouteR);
            loadOfNewRouteS = GetLoadFromNodeList(nodeListOfNewRouteS);

            objValueOfCurrentMove = tempObjValueOfCurrentMove + lengthOfNewRouteR + lengthOfNewRouteS
                + alphaCap * Math.Max(0, loadOfNewRouteR - capacity) + alphaCap * Math.Max(0, loadOfNewRouteS - capacity);

            // update the best move
            if (objValueOfCurrentMove < objValueOfBestMove)
            {
                objValueOfBestMove = objValueOfCurrentMove;
                originalRouteR = routeR;
                originalRouteS = routeS;
                nodeListOfBestNewRouteR = nodeListOfNewRouteR;
                nodeListOfBestNewRouteS = nodeListOfNewRouteS;
                removedLinksOfBestMove = removedLinksOfCurrentMove;
            }
        }

        public static void TryThreeExchange1(Path routeR, double capacity, Link[,] linkMatrix)
        {
            double tempObjValueOfCurrentMove;
            GetTempObjValueOfCurrentMove(routeR, capacity, out tempObjValueOfCurrentMove);

            // store the objective value with penalty of the current move
            double objValueOfCurrentMove;

            int indexOfNodeI = routeR.AllNodes.IndexOf(nodeI);
            int indexOfNodeJ = routeS.AllNodes.IndexOf(nodeJ);

            if ((routeR.AllNodes.Count - indexOfNodeI) < 2)
            {
                return;
            }

            List<Link> otherAddedLinks = new List<Link>();
            otherAddedLinks.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, nodeI.Id]);
            otherAddedLinks.Add(linkMatrix[routeR.AllNodes[indexOfNodeI - 1].Id, routeR.AllNodes[indexOfNodeI + 1].Id]);

            foreach (Link link in otherAddedLinks)
            {
                if (link == null)
                    continue;

                if (tabuLinks.ContainsKey(link) || !candidateLinks.Contains(link))
                    return;
            }


            // create two node list to hold the new routes after performing node exchange mechanisms
            //List<Link> removedLinksOfCurrentMove = new List<Link>();
            List<Node> nodeListOfNewRouteR = new List<Node>();
            List<Node> nodeListOfNewRouteS = new List<Node>();
            double lengthOfNewRouteR;
            double lengthOfNewRouteS;
            double loadOfNewRouteR;
            double loadOfNewRouteS;
            List<Link> removedLinksOfCurrentMove = new List<Link>();

            // add removed links to removeLinksOfCurrentMove
            removedLinksOfCurrentMove.Add(linkMatrix[routeR.AllNodes[indexOfNodeI - 1].Id, nodeI.Id]);
            if (routeR.AllNodes[indexOfNodeI - 1].Id == nodeI.Id)
                Console.WriteLine("ok");
            removedLinksOfCurrentMove.Add(linkMatrix[nodeI.Id, routeR.AllNodes[indexOfNodeI + 1].Id]);
            if (nodeI.Id== routeR.AllNodes[indexOfNodeI + 1].Id)
                Console.WriteLine("ok");
            removedLinksOfCurrentMove.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, nodeJ.Id]);
            if (routeS.AllNodes[indexOfNodeJ - 1].Id==nodeJ.Id)
                Console.WriteLine("ok");


            // three exchange
            // NewRouteR
            for (int i = 0; i < indexOfNodeI; i++)
            {
                nodeListOfNewRouteR.Add(routeR.AllNodes[i]);
            }
            for (int i = indexOfNodeI + 1; i < routeR.AllNodes.Count; i++)
            {
                nodeListOfNewRouteR.Add(routeR.AllNodes[i]);
            }
            // NewRouteS
            for (int i = 0; i < indexOfNodeJ; i++)
            {
                nodeListOfNewRouteS.Add(routeS.AllNodes[i]);
            }

            nodeListOfNewRouteS.Add(routeR.AllNodes[indexOfNodeI]);

            for (int i = indexOfNodeJ; i < routeS.AllNodes.Count; i++)
            {
                nodeListOfNewRouteS.Add(routeS.AllNodes[i]);
            }


            lengthOfNewRouteR = GetLengthFromNodeList(nodeListOfNewRouteR);
            lengthOfNewRouteS = GetLengthFromNodeList(nodeListOfNewRouteS);

            loadOfNewRouteR = GetLoadFromNodeList(nodeListOfNewRouteR);
            loadOfNewRouteS = GetLoadFromNodeList(nodeListOfNewRouteS);

            objValueOfCurrentMove = tempObjValueOfCurrentMove + lengthOfNewRouteR + lengthOfNewRouteS
                + alphaCap * Math.Max(0, loadOfNewRouteR - capacity) + alphaCap * Math.Max(0, loadOfNewRouteS - capacity);

            // update the best move
            if (objValueOfCurrentMove < objValueOfBestMove)
            {
                objValueOfBestMove = objValueOfCurrentMove;
                originalRouteR = routeR;
                originalRouteS = routeS;
                nodeListOfBestNewRouteR = nodeListOfNewRouteR;
                nodeListOfBestNewRouteS = nodeListOfNewRouteS;
                removedLinksOfBestMove = removedLinksOfCurrentMove;
            }
        
        }

        public static void TryThreeExchange2(Path routeR, double capacity, Link[,] linkMatrix)
        {
            double tempObjValueOfCurrentMove;
            GetTempObjValueOfCurrentMove(routeR, capacity, out tempObjValueOfCurrentMove);

            // store the objective value with penalty of the current move
            double objValueOfCurrentMove;

            int indexOfNodeI = routeR.AllNodes.IndexOf(nodeI);
            int indexOfNodeJ = routeS.AllNodes.IndexOf(nodeJ);


            if ((routeR.AllNodes.Count - indexOfNodeI) < 2 || (routeS.AllNodes.Count - indexOfNodeJ) < 3 || indexOfNodeJ == 0)
            {
                return;
            }

            List<Link> otherAddedLinks = new List<Link>();
            otherAddedLinks.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, routeS.AllNodes[indexOfNodeJ + 2].Id]);
            otherAddedLinks.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ + 1].Id, routeR.AllNodes[indexOfNodeI + 1].Id]);

            foreach (Link link in otherAddedLinks)
            {
                if (link == null)
                    continue;

                if (tabuLinks.ContainsKey(link) || !candidateLinks.Contains(link))
                    return;
            }

            // create two node list to hold the new routes after performing node exchange mechanisms
            //List<Link> removedLinksOfCurrentMove = new List<Link>();
            List<Node> nodeListOfNewRouteR = new List<Node>();
            List<Node> nodeListOfNewRouteS = new List<Node>();
            double lengthOfNewRouteR;
            double lengthOfNewRouteS;
            double loadOfNewRouteR;
            double loadOfNewRouteS;
            List<Link> removedLinksOfCurrentMove = new List<Link>();

            // add removed links to removeLinksOfCurrentMove
            removedLinksOfCurrentMove.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ - 1].Id, nodeJ.Id]);
            removedLinksOfCurrentMove.Add(linkMatrix[routeS.AllNodes[indexOfNodeJ + 1].Id, routeS.AllNodes[indexOfNodeJ + 2].Id]);
            removedLinksOfCurrentMove.Add(linkMatrix[nodeI.Id, routeR.AllNodes[indexOfNodeI + 1].Id]);

            // three exchange
            for (int i = 0; i < indexOfNodeJ; i++)
            {
                nodeListOfNewRouteS.Add(routeS.AllNodes[i]);
            }
            for (int i = indexOfNodeJ + 2; i < routeS.AllNodes.Count; i++)
            {
                nodeListOfNewRouteS.Add(routeS.AllNodes[i]);
            }

            for (int i = 0; i < indexOfNodeI + 1; i++)
            {
                nodeListOfNewRouteR.Add(routeR.AllNodes[i]);
            }

            nodeListOfNewRouteR.Add(routeS.AllNodes[indexOfNodeJ]);
            nodeListOfNewRouteR.Add(routeS.AllNodes[indexOfNodeJ + 1]);

            for (int i = indexOfNodeI + 1; i < routeR.AllNodes.Count; i++)
            {
                nodeListOfNewRouteR.Add(routeR.AllNodes[i]);
            }    
      
            lengthOfNewRouteR = GetLengthFromNodeList(nodeListOfNewRouteR);
            lengthOfNewRouteS = GetLengthFromNodeList(nodeListOfNewRouteS);

            loadOfNewRouteR = GetLoadFromNodeList(nodeListOfNewRouteR);
            loadOfNewRouteS = GetLoadFromNodeList(nodeListOfNewRouteS);

            objValueOfCurrentMove = tempObjValueOfCurrentMove + lengthOfNewRouteR + lengthOfNewRouteS
                + alphaCap * Math.Max(0, loadOfNewRouteR - capacity) + alphaCap * Math.Max(0, loadOfNewRouteS - capacity);

            // update the best move
            if (objValueOfCurrentMove < objValueOfBestMove)
            {
                objValueOfBestMove = objValueOfCurrentMove;
                originalRouteR = routeR;
                originalRouteS = routeS;
                nodeListOfBestNewRouteR = nodeListOfNewRouteR;
                nodeListOfBestNewRouteS = nodeListOfNewRouteS;
                removedLinksOfBestMove = removedLinksOfCurrentMove;
            }         
        }
    }
}
